/** 	\file
	\brief Wally - the sensor turret. Scans his surroundings with sensors and reports movement.

	Wally - the sensor turret. Scans his surroundings with sensors and reports movement.
PINs in use:
A0	: IR sensor (in)\n
A1	: line scanner sensor (in)\n
A4	: I2C communication\n
A5	: I2C communication\n
\n
2	: RX pin for XBee shield\n
3	: TX pin for Xbee shield\n
5	: movement sensor (XXXX WHITE?)
7	: PING))) ultrasound sensor (in)\n
8	: sound sensor (in)\n
9	: servo control (out)\n
10	: line scanner clock signal
11	: line scanner sync signal
13	: Laser (high to switch on)

	\addtogroup Wally
	\{
*/

#include <Servo.h>					// the standard servo library
#include <SoftwareSerial.h>			// software serial library for Xbee shield communication on digital pins 2 and 3
#include <Wire.h>					// for i2C communication with RAM and RTC

#include "Wally.h"
#include "i2c_peripherals.h"

#include <RunningMedian.h>			// library to calculate a running median used for sensor measurements

#define NO_PIN_STATE        		///< to indicate that you don't need the pinState (save RAM/speed on PinChangeInt.h)
#define NO_PIN_NUMBER      			///< to indicate that you don't need the arduinoPin (save RAM/speed on PinChangeInt.h)
#define NO_PORTC_PINCHANGES			///< to indicate that port c will not be used for pin change interrupts (save RAM/speed on PinChangeInt.h)

Servo sensorTurret;					///< set a variable to map the servo

uint16_t turretAngle;				///< variable to store the servo position (0 by default)
uint16_t index;						///< the index of the current reading

uint8_t scannerRun;					///< we store 3 full scans from the line scanner for comaparison

uint8_t timeDate[4];				///< Contains the current time and date
uint8_t lastMinute;					///< keeps track of the current minute to time XBee updates

boolean DETECTED;					///< have we found someone?
boolean SOUND;						///< have we heard something?
boolean MOVEMENT;					///< have we seen something

//boolean noisyEnvironment;			///< flag to temporarily disable sound sensor

const boolean DEBUG    =  1;		///< Increase reporting

boolean interruptDisable; 			///< flag to disable/enable interrupts (possibly redundant with capacitors in place to prevent noise from sensors)

// We need to shuffle some or all of this to external RAM, as it's eating up all our RAM - (132)*2B = 264B per array => 528B total
uint16_t measurePING[servoMaxAngle];			///< Array for PING))) measurements. We only measure in 1 degree steps
uint16_t measureIR[servoMaxAngle];				///< Array for IR measurements. We only measure in 1 degree steps

SoftwareSerial xbeeSerial(xbeeRX, xbeeTX);		///< initialize software serial port on pins 2 and 3 for communication with XBee shield


/**
*	\brief The Arduino setup function
*/
void setup()
{
	//sensorTurret.attach(servoPin, servoMIN, servoMAX);		// attach servo to our servoPin
	sensorTurret.attach(servoPin);

	pinMode(soundPin, INPUT);      							// sets the pin for the sound sensor as input
	pinMode(movementPin, INPUT);      						// sets the pin for the movement sensor as input

	pinMode(laserPin, OUTPUT);
	digitalWrite(laserPin, LOW);							// turn laser off

	pinMode(LSpin, INPUT); 									// sets the pin for the line scanner as input
	pinMode(LSclockPin, OUTPUT);
	pinMode(LSsyncPin, OUTPUT);

	Serial.begin(115200);									// initialize the serial port
	Serial.println("");
	Serial.println(F(" *** I LIVE AGAIN ***"));				// Note! The F() reads from flash and saves RAM

	xbeeSerial.begin(9600);									// sets software serial speed to 9600 - must match Xbee hardware config @todo Check if we can increase it about without trouble from heat
	Wire.begin();											// masters can but doesn't have to have an address
	//setRTCtime();											// ONLY enable this if we want to RESET the clock


	// sensorSweep(0, servoMaxAngle,'r');					// Performing initial sweep to set up map of surroundings
	sensorTurret.write(0);	// rotate servo back to zero  position
	delay(500);	// avoid the servo sound triggering from position reset

	xbeeSerial.print(F("{s1:c0:WALLY READY}\n"));		// report in - note the newline to ease reading on PC
	//getRTCtime(timeDate);
	//lastMinute = timeDate[3];

}

/**
*	\brief The Arduino main loop
*/
void loop()
{
/////////////////////////  TEST AREA //////////////////////////////////

writeRAM(1, 10);
writeRAM(2, 50);

delay(1000);
Serial.println(readRAM(2));
Serial.println(readRAM(1));
Serial.println(readRAM(10));

while (1) {};

/*
while(1) {
	getCamera();
	delay(100);
}
/*
	getRTCtime(timeDate);
	if (timeDate[3] > lastMinute || (timeDate[3] == 0 && lastMinute == 59)) {	// a minute has passed
			xbeeSerial.print("{s1:c5:UPDATING TIME"); //debug debug
			xbeeSerial.print("{s1:c4:");
			xbeeSerial.print(timeDate[0]);
			xbeeSerial.print(timeDate[1]);
			xbeeSerial.print(timeDate[2]);
			xbeeSerial.print(timeDate[3]);
			xbeeSerial.print("}\n");
			lastMinute = timeDate[3];
			if (DEBUG) Serial.print("minute ");
			if (DEBUG) Serial.print(timeDate[3]);
		}

*/
	/* Calibration - if uncommented wil place servo in center position
	while (1) {
		sensorTurret.write(servoMaxAngle/2);
		Serial.println("Sitting in center position....");
	} */
////////////////////////  END OF TEST AREA ////////////////////////////


	//if(digitalRead(movementPin) == HIGH) detectedMovement();
	if(digitalRead(soundPin) == HIGH) detectedSound();

	MOVEMENT = LOW ; // debug debug - too sensitive for testing, can't even draw breath in the same room
	//SOUND = LOW ; // debug debug

	/*
	 *	This is the main block that deal with scanning and reporting once we've heard a sound or noticed movement.
	 *
	 */
	if(SOUND == HIGH || MOVEMENT == HIGH) {	// Start everything when we hear or see movement (direction is still unknown)
		if(DEBUG) {
			Serial.print(F("We detected "));
			if (SOUND == HIGH) Serial.println(F("sound"));
			if (MOVEMENT == HIGH) Serial.println(F("movement"));
		}
		xbeeSerial.print(F("{s1:c0:SCANNING}\n"));

		/*
		 *	Here we scan for a target and we take appropriate actions during the scan.
		 *	We only return from the the sensor sweep once the movement has been dealt with
		 *	We should make it smarter - like checking only every 10degree angle as it might be too slow when checking every step
		 */
		sensorTurret.attach(servoPin);//, servoMIN, servoMAX);		// attach servo to our servoPin
		sensorSweep(0, servoMaxAngle,'s');			// sweeping left to right
		//sensorSweep(ceil(servoMaxAngle/2), servoMaxAngle,'s');
		//sensorSweep(ceil(servoMaxAngle/2), 0, 's');
		sensorTurret.write(0);	// rotate servo back to zero  position
		delay(500);	// wait second  and a half so the sound sensor doesn't pick up the servo noise
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

		if(!DETECTED) {	// false: alert, we heard/saw something, but nothing showed up on the sweep
			if (DEBUG) Serial.println(F("Nothing found during sweep - returning to passive mode"));
			xbeeSerial.print(F("{s1:c0:FALSE ALERT}\n"));	// testing
			} else {	// threat found and dealt with @note: separate option for target tracked but eluded us?
			if (DEBUG) Serial.println(F("Threat found and neutralized - retuning to normal operations"));
			xbeeSerial.print(F("{s1:c0:NORMALIZING}\n"));	// testing
			}

		DETECTED = SOUND = MOVEMENT = 0;	// resetting flags
	}

}


uint8_t sensorSweep(uint8_t startAngle, uint8_t endAngle, uint8_t mode)
{
	if (DEBUG) Serial.println(F("Starting sensor sweep"));

	uint16_t distPING;
	uint16_t distIR;

	uint8_t leftBorderMovement;	// left extreme of moving object
	uint8_t rightBorderMovement;// right extreme of moving object
	uint8_t centerMovement;		// center of moving object
	uint8_t widthMovement;		// the width of the object in question
	uint8_t distanceMovement;	// distance to the moving object

	float tolerancePING = 0.15;	// 15% deviation allowed
	float toleranceIR = 0.15;	// 15% deviation allowed

	int8_t direction = 1;		// direction of servo: 1=right, -1=left
	if(startAngle > endAngle) direction = -1;

 	sensorTurret.write(startAngle);	// we move turret to start position and ...
 	delay(1000);						// ...give it time to get there before we begin


	for(turretAngle = startAngle; turretAngle != endAngle; turretAngle += direction) {	// Start rotation
		sensorTurret.write(turretAngle);	// rotate servo
		distPING = readSensor(sensorPING);	// read PING))) sensor
		distIR = readSensor(sensorIR);		// read IR sensor

		switch(mode) {

			case 'r': {	// we're building up the reference array - important to avoid movement in this run!
				measurePING[turretAngle] = distPING;	// Storing distance readings into the reference
				measureIR[turretAngle] = distIR;		// array for later runs
				if (DEBUG) {
					Serial.print("Angle: ");
					Serial.print(turretAngle);
					Serial.print(" =    PING))) dist: ");
					Serial.print(distPING);
					Serial.print("            IR dist: ");
					Serial.print(distIR);
					Serial.println();
					}

			}
			break;

			case 's': {						// we compare readings to the reference array - this is normal scan mode
				if (turretAngle < startAngle + (direction * 2)) break;	// we need to compare two values, skipping first reading

				// The logic here is that single spikes are common but double very rare, so we compare this angle's and previous angles'
				// distance readings  before concluding that we have a *real* deviation
				if (   ((distPING > measurePING[turretAngle] + (measurePING[turretAngle] * tolerancePING) ||		// checking for any
						distPING < measurePING[turretAngle] - (measurePING[turretAngle] * tolerancePING)) 			// *significant* deviation
							&&																						// for both sensors at same time
						(distIR > measureIR[turretAngle] + (measureIR[turretAngle] * toleranceIR) ||				// between stored and
						distIR < measureIR[turretAngle] - (measureIR[turretAngle] * toleranceIR)) )					// current value
					&&
					   ((distPING > measurePING[turretAngle - direction] + (measurePING[turretAngle - direction] * tolerancePING) ||
						distPING < measurePING[turretAngle - direction] - (measurePING[turretAngle - direction] * tolerancePING))
							&&
						(distIR > measureIR[turretAngle - direction] + (measureIR[turretAngle - direction] * toleranceIR) ||
						distIR < measureIR[turretAngle - direction] - (measureIR[turretAngle - direction] * toleranceIR)) )
						) {

					// We have detected movement at angle turretAngle!
					if (DEBUG) {
					Serial.print("Angle: ");
					Serial.print(turretAngle);
					Serial.print(" = PING))) dist: ");
					Serial.print(distPING);
					Serial.print(" vs. original ");
					Serial.print(measurePING[turretAngle]);
					Serial.print(" and IR dist: ");
					Serial.print(distIR);
					Serial.print(" vs. original ");
					Serial.print(measureIR[turretAngle]);
					Serial.println();
					}

					DETECTED = 1;
					digitalWrite(laserPin, HIGH); 	// enabling laser to indicate we have found something
					xbeeSerial.print("{s1:c5:TARGET FOUND}\n");

					if (DEBUG) {
						Serial.print(F("We found one border at "));
						Serial.println(turretAngle);
						}

					if (direction == 1) {
						leftBorderMovement = turretAngle;	// we're going left to right - this is the left border
						rightBorderMovement = sensorSweep(leftBorderMovement, servoMaxAngle, 't');
					}

					if (direction == -1) {
						rightBorderMovement = turretAngle;	// we're going right to left - this is the right border
						leftBorderMovement = sensorSweep(rightBorderMovement, 0, 't');
					}

				} else {
				// We didn't find anything - keep on scannin'
				}

			}	// end of case 's'
			break;

			case 't': {		// THIS IS ONLY CALLED RECURSIVELY - looking for the other border of a moving object
					// We start a separate scan going further in one direction until we find reading that match the template again
					while(	distPING > measurePING[turretAngle] + (measurePING[turretAngle] * tolerancePING) ||
							distPING < measurePING[turretAngle] - (measurePING[turretAngle] * tolerancePING) ||
							distIR > measureIR[turretAngle] + (measureIR[turretAngle] * toleranceIR) ||
							distIR < measureIR[turretAngle] - (measureIR[turretAngle] * toleranceIR) ) {

						// We take over the rotation here from the main for loop and then exists the function with a return value
						turretAngle += direction;				// continuing leftwards rotation until we find known readings
						measurePING[turretAngle] = distPING;	// Storing distance readings into the reference
						measureIR[turretAngle] = distIR;		// array for later runs
					}
					// We have now found the first angle that matches the background again and we're done here
					return turretAngle;
			}	// end of case 't'
		}	// End of switch statement
	}	// End of for loop
   	digitalWrite(laserPin, LOW);		// disable laser to indicate sweep complete
}	// end of function

//Read sensor and report back distance.
uint16_t readSensor(uint8_t sensor)
{
	delay(10);		// Give the sensors time to catch their breath and the rig to stop moving

	float distance;
	switch(sensor)
	{
		/*
			Parallax PING))) is a very accurate binary distance sensor, but has a wide field of view.
			For unknown reason it's consistently 5cm short. I've manually compensated for that in the code
			Distance range: 20cm - 300cm
		*/
		case sensorPING:
		{
			// NOTE: We do not do a median calculation on the PING))) as we *hope* it doesn't give us spikes.
			//	If we see spikes, we need to do minimum 3 iteration and calculate median
			long duration;

			// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
			// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
			pinMode(PINGpin, OUTPUT);
			digitalWrite(PINGpin, LOW);
			delayMicroseconds(2);
			digitalWrite(PINGpin, HIGH);
			delayMicroseconds(5);
			digitalWrite(PINGpin, LOW);

			//	The same pin is used to read the signal from the PING))):
			//	a HIGH pulse whose duration is the time (in microseconds) from the sending of the ping to the
			//	reception of its echo off of an object.
			pinMode(PINGpin, INPUT);
			duration = pulseIn(PINGpin, HIGH);

			// Convert the time into a distance. The speed of sound is 340 m/s or 29 microseconds per centimeter.
			// The ping travels out and back, so to find the distance of the object we take half of the distance travelled.
			distance = (duration / 29 / 2) + 5;	// Comes up 5cm short, no idea why. Manually fixed here

    //  if(DEBUG) Serial.print(F("PING:"));			// Debug debug
		//	if (DEBUG) Serial.println(distance);						// Debug debug


		//	if (distance > 300) distance = 300;	// we get crazy readings over 300cm
		//	if (distance < 20) distance = 20; 	// ditto for less than 30cm
			break;
		}

		/*
			Sharp Rangefinder (GP2Y0A02YK0F) is a slightly less accurate analog distance monitor, but has a
			very narrow field of view. Distance range: 30cm - 180cm : all over 180cm gets cut down to 180cm
		*/
		case sensorIR:
		{
			uint8_t iterations = 5;
			RunningMedian samples = RunningMedian();	// We now create an object to get median value of samples

			for (uint8_t i = 0; i < iterations; i++)	// Find the median from several readings
			{
				samples.add(analogRead(IRpin));
			}
			distance = 10650.08 * pow(samples.getMedian(), -0.950) - 10;	// Tweaked for my IR sensor
      if(!DEBUG) Serial.print(F("SHARP:"));			// Debug debug
			if (!DEBUG) Serial.println(distance);						// Debug debug
			if (distance > 250) distance = 250;	// we get crazy readings over 180cm
			if (distance < 30) distance = 30; 	// ditto for less than 30cm
			break;
		}

	}
	return((int)distance);

}

void getCamera()
{

	digitalWrite(laserPin, HIGH);							// turn laser on

	Serial.println(0);
	uint16_t utime;
	uint16_t exposure = 5000;	// set by trial & error basically - 5000 seems about right
	uint8_t lightVal[128];

	digitalWrite(LSclockPin, LOW);
	pinMode(LSsyncPin, OUTPUT);
	digitalWrite(LSsyncPin, HIGH);
	digitalWrite(LSclockPin, HIGH);
	digitalWrite(LSsyncPin, LOW);
	utime = micros();
	digitalWrite(LSclockPin, LOW);

	for (int j = 0; j < 128; j++) {
		digitalWrite(LSclockPin, HIGH);
		digitalWrite(LSclockPin, LOW);
	}

	delayMicroseconds(exposure);
	digitalWrite(LSsyncPin, HIGH);
	digitalWrite(LSclockPin, HIGH);
	digitalWrite(LSsyncPin, LOW);
	utime = micros() - utime;
	digitalWrite(LSclockPin, LOW);

	for (int j = 0; j < 128; j++) {
    	delayMicroseconds(20);
		writeRAM(j + (scannerRun * 128), analogRead(LSpin));		// we put the reading directly into external RAM
		if (DEBUG) Serial.println(readRAM(j + (scannerRun * 128)));
		//Serial.print(" ");

    	digitalWrite(LSclockPin, HIGH);
    	digitalWrite(LSclockPin, LOW);
    }
	//Serial.println();

	//delayMicroseconds(20);
	/*for (int j = 0; j < 128; j++) {
		itoa(lightVal[j], 'S');
	}

	itime = (int)utime;
	itoa(itime, 'S');*/

	if (scannerRun++ > 2) scannerRun = 0;	// the counter of how which run we're doing

}

/**
\brief Checks if we are right now hearing anything on the sound sensor.
\return 0==Nothing to report, 1==Sound detected
*/
void detectedSound(void)
{
	// Note: the input pulse from the sensor should be about 50ms
	Serial.println(F("SOUND DETECTED!"));
	xbeeSerial.print("{s1:c5:SOUND DETECTED}\n");
	SOUND = 1;
}

/**
\brief Checks if we are right now are seeing anything via the hall sensor.
\return 0==Nothing to report, 1==Movement detected
*/
void detectedMovement(void)
{
	// Note: the input pulse from the sensor should be about 50ms
	Serial.println(F("MOVEMENT DETECTED!"));
	xbeeSerial.print("{s1:c5:MOVEMENT DETECTED}\n");
	MOVEMENT = 1;
}

/// \}
